#include "Matrix.h"

/// Default constructor. Everything is qual to zero
template<class T> mat4<T>::mat4() :
	m11(0),m12(0),m13(0),m14(0),
	m21(0),m22(0),m23(0),m24(0),
	m31(0),m32(0),m33(0),m34(0),
	m41(0),m42(0),m43(0),m44(0) { }

/// Entry by entry constructor. Row-major
/** @param n11-n44 - entries */
template<class T> mat4<T>::mat4(		T n11, T n12, T n13, T n14,
												T n21, T n22, T n23, T n24,
												T n31, T n32, T n33, T n34,
												T n41, T n42, T n43, T n44	 ) :
	m11(n11),m12(n12),m13(n13),m14(n14),
	m21(n21),m22(n22),m23(n23),m24(n24),
	m31(n31),m32(n32),m33(n33),m34(n34),
	m41(n41),m42(n42),m43(n43),m44(n44) { }

/// Enter from array of size [16]. Row-major
/** @param data - array holder */
template<class T> mat4<T>::mat4( const T *data ) :
	m11(data[ 0]), m12(data[ 1]), m13(data[ 2]), m14(data[ 3]),
	m21(data[ 4]), m22(data[ 5]), m23(data[ 6]), m24(data[ 7]),
	m31(data[ 8]), m32(data[ 9]), m33(data[10]), m34(data[11]),
	m41(data[12]), m42(data[13]), m43(data[14]), m44(data[15]) { }

/// Copy (from float)
/** @param m3x3 - external matrix */
template<class T> mat4<T>::mat4( const mat3f &m3x3 ) :
	m11((T)m3x3.m11), m12((T)m3x3.m12), m13((T)m3x3.m13), m14(0),
	m21((T)m3x3.m21), m22((T)m3x3.m22), m23((T)m3x3.m23), m24(0),
	m31((T)m3x3.m31), m32((T)m3x3.m32), m33((T)m3x3.m33), m34(0),
	m41(0), m42(0), m43(0), m44(1) { }

/// Copy (from double)
/** @param m4x4 - external matrix */
template<class T> mat4<T>::mat4( const mat3d &m3x3 ) :
	m11((T)m3x3.m11), m12((T)m3x3.m12), m13((T)m3x3.m13), m14(0),
	m21((T)m3x3.m21), m22((T)m3x3.m22), m23((T)m3x3.m23), m24(0),
	m31((T)m3x3.m31), m32((T)m3x3.m32), m33((T)m3x3.m33), m34(0),
	m41(0), m42(0), m43(0), m44(1) { }

/// Copy (from float)
/** @param m4x4 - external matrix */
template<class T> mat4<T>::mat4( const mat4f &m4x4 ) :
	m11((T)m4x4.m11), m12((T)m4x4.m12), m13((T)m4x4.m13), m14((T)m4x4.m14),
	m21((T)m4x4.m21), m22((T)m4x4.m22), m23((T)m4x4.m23), m24((T)m4x4.m24),
	m31((T)m4x4.m31), m32((T)m4x4.m32), m33((T)m4x4.m33), m34((T)m4x4.m34),
	m41((T)m4x4.m41), m42((T)m4x4.m42), m43((T)m4x4.m43), m44((T)m4x4.m44) { }

/// Copy (from double)
/** @param m4x4 - external matrix */
template<class T> mat4<T>::mat4(	const mat4d	&m4x4	) :
	m11((T)m4x4.m11), m12((T)m4x4.m12), m13((T)m4x4.m13), m14((T)m4x4.m14),
	m21((T)m4x4.m21), m22((T)m4x4.m22), m23((T)m4x4.m23), m24((T)m4x4.m24),
	m31((T)m4x4.m31), m32((T)m4x4.m32), m33((T)m4x4.m33), m34((T)m4x4.m34),
	m41((T)m4x4.m41), m42((T)m4x4.m42), m43((T)m4x4.m43), m44((T)m4x4.m44) { }

/// Construct from rows
/** @param row 1-4 - row vectors. row 4 default is vec4( 0, 0, 0, 1 ) */
template<class T> mat4<T>::mat4(	const vec4<T> &row0,
											const vec4<T> &row1,
											const vec4<T> &row2,
											const vec4<T> &row3 ) :
	m11(row0.x), m12(row0.y), m13(row0.z), m14(row0.w),
	m21(row1.x), m22(row1.y), m23(row1.z), m24(row1.w),
	m31(row2.x), m32(row2.y), m33(row2.z), m34(row2.w),
	m41(row3.x), m42(row3.y), m43(row3.z), m44(row3.w) { }

/// Construct from coordinate system unit vectors, position and scale
/**	@param - right, up, view - vectors of orientation
		@param - pos - translation. The default is vec3(0,0,0)
		@param - scale - scaling. The default is vec3(1,1,1) */
template<class T> mat4<T>::mat4(	const vec3<T> &right,
											const vec3<T> &up,
											const vec3<T> &view,
											const vec3<T> &pos,
											const vec3<T> &scale ) :
	m11(right.x*scale.x),	m12(up.x),				m13(view.x),			m14(pos.x),
	m21(right.y),				m22(up.y*scale.y),	m23(view.y),			m24(pos.y),
	m31(right.z),				m32(up.z),				m33(view.z*scale.z), m34(pos.z),
	m41(0),						m42(0),					m43(0),					m44(1)		{ }

/// Construct from quaternion, scaling and position
/**	@param - orient - orientation quaternion
		@param - scale - scaling. The default is vec3(1,1,1)
		@param - pos - translation. The default is vec3(0,0,0) */
template<class T> mat4<T>::mat4(	const quat<T> &orient,
											const vec3<T> &scale,
											const vec3<T> &pos ) {
	quat<T> q = orient.NormalizedQ();
	T x2 = q.x * q.x;		T y2 = q.y * q.y;		T z2 = q.z * q.z;
	T xy = q.x * q.y;		T xz = q.x * q.z;		T yz = q.y * q.z;
	T wx = q.w * q.x;		T wy = q.w * q.y;		T wz = q.w * q.z;

	m11 = ((T)1.0 - (T)2.0 * (y2 + z2))*scale.x;	m12 = (T)2.0 * (xy - wz);							m13 = (T)2.0 * (xz + wy);							m14 = pos.x;
	m21 = (T)2.0 * (xy + wz);							m22 = ((T)1.0 - (T)2.0 * (x2 + z2))*scale.y;	m23 = (T)2.0 * (yz - wx);							m24 = pos.y;
	m31 = (T)2.0 * (xz - wy);							m32 = (T)2.0 * (yz + wx);							m33 = ((T)1.0 - (T)2.0 * (x2 + y2))*scale.z;	m34 = pos.z;
	m41 = 0;													m42 = 0;													m43 = 0;													m44 = (T)1;
}



/// Multiplication operator (matrix * matrix)
/**	@param m2 - external matrix
		@return mat4 */
template<class T> mat4<T> mat4<T>::operator * ( const mat4<T> &m2	) const {
	mat4<T> r;
	r.m11 = m11 * m2.m11 + m12 * m2.m21 + m13 * m2.m31 + m14 * m2.m41;
	r.m12 = m11 * m2.m12 + m12 * m2.m22 + m13 * m2.m32 + m14 * m2.m42;
	r.m13 = m11 * m2.m13 + m12 * m2.m23 + m13 * m2.m33 + m14 * m2.m43;
	r.m14 = m11 * m2.m14 + m12 * m2.m24 + m13 * m2.m34 + m14 * m2.m44;

	r.m21 = m21 * m2.m11 + m22 * m2.m21 + m23 * m2.m31 + m24 * m2.m41;
	r.m22 = m21 * m2.m12 + m22 * m2.m22 + m23 * m2.m32 + m24 * m2.m42;
	r.m23 = m21 * m2.m13 + m22 * m2.m23 + m23 * m2.m33 + m24 * m2.m43;
	r.m24 = m21 * m2.m14 + m22 * m2.m24 + m23 * m2.m34 + m24 * m2.m44;

	r.m31 = m31 * m2.m11 + m32 * m2.m21 + m33 * m2.m31 + m34 * m2.m41;
	r.m32 = m31 * m2.m12 + m32 * m2.m22 + m33 * m2.m32 + m34 * m2.m42;
	r.m33 = m31 * m2.m13 + m32 * m2.m23 + m33 * m2.m33 + m34 * m2.m43;
	r.m34 = m31 * m2.m14 + m32 * m2.m24 + m33 * m2.m34 + m34 * m2.m44;

	r.m41 = m41 * m2.m11 + m42 * m2.m21 + m43 * m2.m31 + m44 * m2.m41;
	r.m42 = m41 * m2.m12 + m42 * m2.m22 + m43 * m2.m32 + m44 * m2.m42;
	r.m43 = m41 * m2.m13 + m42 * m2.m23 + m43 * m2.m33 + m44 * m2.m43;
	r.m44 = m41 * m2.m14 + m42 * m2.m24 + m43 * m2.m34 + m44 * m2.m44;

	return r;
}

/// Multiplication operator (matrix * vec3)
/**	@param v - 3D vector
		@return 3D vector */
template<class T> vec3<T> mat4<T>::operator * ( const vec3<T> &v ) const {
	return vec3<T>(
		m11 * v.x + m12 * v.y + m13 * v.z + m14,
		m21 * v.x + m22 * v.y + m23 * v.z + m24,
		m31 * v.x + m32 * v.y + m33 * v.z + m34	);
}

/// Multiplication operator (matrix * vec4)
/**	@param v - 4D vector
		@return 4D vector */
template<class T> vec4<T> mat4<T>::operator * ( const vec4<T> &v ) const {
   return vec4<T>(
      m11 * v.x + m12 * v.y + m13 * v.z + m14 * v.w,
      m21 * v.x + m22 * v.y + m23 * v.z + m24 * v.w,
      m31 * v.x + m32 * v.y + m33 * v.z + m34 * v.w,
      m41 * v.x + m42 * v.y + m43 * v.z + m44 * v.w	);
}

/// Multiplication operator (matrix * scalar)
/**	@param scalar - 1D variable
		@return mat4 */
template<class T> mat4<T> mat4<T>::operator * ( const T &scalar ) const {
   return mat4<T>(
      m11 * scalar, m12 * scalar, m13 * scalar, m14 * scalar,
      m21 * scalar, m22 * scalar, m23 * scalar, m24 * scalar,
      m31 * scalar, m32 * scalar, m33 * scalar, m34 * scalar,
      m41 * scalar, m42 * scalar, m43 * scalar, m44 * scalar	);
}

/// Addition operator (matrix + matrix)
/** @param m2 - external mat4
	 @return mat4 */
template<class T> mat4<T> mat4<T>::operator + ( const mat4<T> &m2	) const {
   return mat4<T>(
      m11 + m2.m11, m12 + m2.m12, m13 + m2.m13, m14 + m2.m14,
      m21 + m2.m21, m22 + m2.m22, m23 + m2.m23, m24 + m2.m24,
      m31 + m2.m31, m32 + m2.m32, m33 + m2.m33, m34 + m2.m34,
      m41 + m2.m41, m42 + m2.m42, m43 + m2.m43, m44 + m2.m44	);
}

/// Subtraction operator (matrix - matrix)
/**	@param m2 - external mat4
		@return temporary mat4 */
template<class T> mat4<T> mat4<T>::operator - ( const mat4<T> &m2	) const {
   return mat4<T>(
      m11 - m2.m11, m12 - m2.m12, m13 - m2.m13, m14 - m2.m14,
      m21 - m2.m21, m22 - m2.m22, m23 - m2.m23, m24 - m2.m24,
      m31 - m2.m31, m32 - m2.m32, m33 - m2.m33, m34 - m2.m34,
      m41 - m2.m41, m42 - m2.m42, m43 - m2.m43, m44 - m2.m44	);
}
/// Self-multiplication operator (matrix *= matrix)
/**	@param m2 - external mat4
		@return temporary mat4 */
template<class T> mat4<T> mat4<T>::operator *= ( const mat4<T> &m2 ) const {
	return *this * m2;
}

/// Self-multiplication operator (matrix *= scalar)
/**	@param scalar - 1D var
		@return temporary mat4 */
template<class T> mat4<T> mat4<T>::operator *= ( const T &scalar ) const {
	return *this * scalar;
}

/// Equality operator (matrix == matrix)
/**	@param m2 - external mat4
		@return true if equal */
template<class T> bool mat4<T>::operator == ( const mat4<T> &m2 ) const {
	if(   m11 == m2.m11	&&		m12 == m2.m12	&&		m13 == m2.m13	&&		m14 == m2.m14	&&
			m21 == m2.m21	&&		m22 == m2.m22	&&		m23 == m2.m23	&&		m24 == m2.m24	&&
			m31 == m2.m31	&&		m32 == m2.m32	&&		m33 == m2.m33	&&		m34 == m2.m34	&&
			m41 == m2.m41	&&		m42 == m2.m42	&&		m43 == m2.m43	&&		m44 == m2.m44	){
		return true;
	}
	return false;
}

template<class T> void mat4<T>::operator  =	( const mat3<T> &m2	) {
	m11 = m2.m11;
	m12 = m2.m12;
	m13 = m2.m13;
	m21 = m2.m21;
	m22 = m2.m22;
	m23 = m2.m23;
	m31 = m2.m31;
	m32 = m2.m32;
	m33 = m2.m33;
}

/// Non-equality operator (matrix != matrix)
/**	@param m2 - external mat4
		@return true if not equal */
template<class T> bool mat4<T>::operator != ( const mat4<T> &m2 ) const {
	if(	m11 != m2.m11	||		m12 != m2.m12	||		m13 != m2.m13	||		m14 != m2.m14	||
			m21 != m2.m21	||		m22 != m2.m22	||		m23 != m2.m23	||		m24 != m2.m24	||
			m31 != m2.m31	||		m32 != m2.m32	||		m33 != m2.m33	||		m34 != m2.m34	||
			m41 != m2.m41	||		m42 != m2.m42	||		m43 != m2.m43	||		m44 != m2.m44	){
		return true;
	}
	return false;
}

/// Row-major (Default) array filling. Row-major
/** @param *&array - holder to allocated array of size [16] */
template<class T> void mat4<T>::GetArray( T *arr ) const {
	arr[0]	= m11;	arr[1]	= m12;	arr[2]	= m13;	arr[3]	= m14;
	arr[4]	= m21;	arr[5]	= m22;	arr[6]	= m23;	arr[7]	= m24;
	arr[8]	= m31;	arr[9]	= m32;	arr[10]	= m33;	arr[11]	= m34;
	arr[12]	= m41;	arr[13]	= m42;	arr[14]	= m43;	arr[15]	= m44;
}

/// Column-major array filling. Column-major
/** @param *&array - holder to allocated array of size [16] */
template<class T> void mat4<T>::GetArrayCM( T *arr ) const {
	arr[0]	= m11;	arr[1]	= m21;	arr[2]	= m31;	arr[3]	= m41;
	arr[4]	= m12;	arr[5]	= m22;	arr[6]	= m32;	arr[7]	= m42;
	arr[8]	= m13;	arr[9]	= m23;	arr[10]	= m33;	arr[11]	= m43;
	arr[12]	= m14;	arr[13]	= m24;	arr[14]	= m34;	arr[15]	= m44;
}

/// Row-major (Default) array filling. Row-major
/** @param *&array - holder to allocated array of size [16] */
template<class T> void mat4<T>::GetArrayRM( T *arr ) const {
	GetArray( arr );
}

/// Get row as vec4
/**	 @param row - row index row=[0;3]. Invalid row returns ZEROVEC
		@return vec4 */
template<class T> vec4<T> mat4<T>::GetRow( const uint16 &row ) const {
	if( row == 0 )
		return vec4<T>( m11, m12, m13, m14 );
	else if( row == 1 )
		return vec4<T>( m21, m22, m23, m24 );
	else if( row == 2 )
		return vec4<T>( m31, m32, m33, m34 );
	else if( row == 3 )
		return vec4<T>( m41, m42, m43, m44 );
	else
		return vec4<T>( 0.f );
}

/// Get column as vec4
/**	@param col - column index col=[0;3]. Invalid column returns ZEROVEC
		@return vec4 */
template<class T> vec4<T> mat4<T>::GetCol( const uint16 &col ) const {
	if( col == 0 )
		return vec4<T>( m11, m21, m31, m41 );
	else if( col == 1 )
		return vec4<T>( m12, m22, m32, m42 );
	else if( col == 2 )
		return vec4<T>( m13, m23, m33, m43 );
	else if( col == 3 )
		return vec4<T>( m14, m24, m34, m44 );
	else
		return vec4<T>( 0.f );
}

/// Get EULER angles from rotational matrix
/**	@return vec3 with turn, yaw, roll angles
		@warning matrix must be set as rotational before use */
template<class T> vec3<T> mat4<T>::GetEuler( ) {
	T D,C;
	T angle_y = D =  asin( m31);				/* Calculate Y-axis angle */
	T angle_x, angle_z;
	C           =  cos( angle_y );
	angle_y    =  r2d( angle_y );
	if ( fabs( C ) > (T)0.005 ){					/* Gimball lock? */
      T trx      =  m33 / C;							/* No, so get X-axis angle */
      T tryy      = -m23  / C;
      angle_x  =  r2d( atan2( tryy, trx ) );
      trx      =  m11 / C;							/* Get Z-axis angle */
      tryy      = -m12 / C;
      angle_z  =  r2d( atan2( tryy, trx ) );
   }
	else{													/* Gimball lock has occurred */
      angle_x  =  0;									/* Set X-axis angle to zero */
      T trx      =  m22;								/* And calculate Z-axis angle */
      T tryy      =  m21;
      angle_z  =  r2d( atan2( tryy, trx ) );
   }

   /* return only positive angles in [0,360] */
   if( angle_x < 0 ) angle_x += (T)360;
   if( angle_y < 0 ) angle_y += (T)360;
   if( angle_z < 0 ) angle_z += (T)360;

	return vec3<T>( angle_x, angle_y, angle_z );
}

/// Get transpose of this matrix
/**	@return mat4 */
template<class T> mat4<T> mat4<T>::Transposed( ) const {
	return mat4<T>(	m11, m21, m31, m41,
							m12, m22, m32, m42,
							m13, m23, m33, m43,
							m14, m24, m34, m44	);
}

/// Get the inversed of that matrix
/**	@return new inversed equivalent of this matrix */
template<class T> mat4<T> mat4<T>::Inversed( ) const {
	T fA0 = m11*m22 - m21*m12;
	T fA1 = m11*m32 - m31*m12;
	T fA2 = m11*m42 - m41*m12;
	T fA3 = m21*m32 - m31*m22;
	T fA4 = m21*m42 - m41*m22;
	T fA5 = m31*m42 - m41*m32;
	T fB0 = m13*m24 - m23*m14;
	T fB1 = m13*m34 - m33*m14;
	T fB2 = m13*m44 - m43*m14;
	T fB3 = m23*m34 - m33*m24;
	T fB4 = m23*m44 - m43*m24;
	T fB5 = m33*m44 - m43*m34;

	T fDet = fA0*fB5-fA1*fB4+fA2*fB3+fA3*fB2-fA4*fB1+fA5*fB0;
	if ( abs(fDet) <= EPS )
		return mat4r();	// return null matrix. inversion is impossible

	mat4<T> kInv;
	kInv.m11 = + m22*fB5 - m32*fB4 + m32*fB3;
	kInv.m12 = - m12*fB5 + m32*fB2 - m32*fB1;
	kInv.m13 = + m12*fB4 - m22*fB2 + m32*fB0;
	kInv.m14 = - m12*fB3 + m22*fB1 - m32*fB0;
	kInv.m21 = - m21*fB5 + m31*fB4 - m41*fB3;
	kInv.m22 = + m11*fB5 - m31*fB2 + m41*fB1;
	kInv.m23 = - m11*fB4 + m21*fB2 - m41*fB0;
	kInv.m24 = + m11*fB3 - m21*fB1 + m31*fB0;
	kInv.m31 = + m24*fA5 - m34*fA4 + m44*fA3;
	kInv.m32 = - m14*fA5 + m34*fA2 - m44*fA1;
	kInv.m33 = + m14*fA4 - m24*fA2 + m44*fA0;
	kInv.m34 = - m14*fA3 + m24*fA1 - m34*fA0;
	kInv.m41 = - m23*fA5 + m33*fA4 - m43*fA3;
	kInv.m32 = + m13*fA5 - m33*fA2 + m43*fA1;
	kInv.m43 = - m13*fA4 + m23*fA2 - m43*fA0;
	kInv.m44 = + m13*fA3 - m23*fA1 + m33*fA0;

	// may be problematic. wrong order? no, it seem all is okay [tested]
	T fInvDet = ((T)1.0)/fDet;
	kInv.m11 *= fInvDet;
	kInv.m21 *= fInvDet;
	kInv.m31 *= fInvDet;
	kInv.m41 *= fInvDet;
	kInv.m12 *= fInvDet;
	kInv.m22 *= fInvDet;
	kInv.m32 *= fInvDet;
	kInv.m42 *= fInvDet;
	kInv.m13 *= fInvDet;
	kInv.m23 *= fInvDet;
	kInv.m33 *= fInvDet;
	kInv.m43 *= fInvDet;
	kInv.m14 *= fInvDet;
	kInv.m24 *= fInvDet;
	kInv.m34 *= fInvDet;
	kInv.m44 *= fInvDet;

	return kInv;
}

/// Set translational part of the matrix
/** @param v - 3D vector */
template<class T> void mat4<T>::SetTranslate( const vec3<T> &v ) {
	m14 = v.x;	m24 = v.y;	m34 = v.z;
}

/// Set translational part of the matrix
/** @param v - 3D vector */
template<class T> void mat4<T>::ApplyTranslation( const vec3<T> &v ) {
	m14 += v.x;	m24 += v.y;	m34 += v.z;
}

/// Get translation part from any matrix
/** @return vec3 coordinates of translation */
template<class T> vec3<T> mat4<T>::GetTranslate( ) const {
	return vec3<T>( m14, m24, m34 );
}

/// SETUP MATRIX AS TRANSLATIONAL (using vector)
/** @param vec3 - 3D coordinates */
template<class T> void mat4<T>::Translational( const vec3<T> &v ) {
	Identity();
	SetTranslate( v );
}

/// SETUP MATRIX AS TRANSLATIONAL	(using XYZ entries)
/** @param tx, ty, tz - entries */
template<class T> void mat4<T>::Translational( const T &tx, const T &ty, const T &tz ) {
	Identity();
	SetTranslate( vec3<T>(tx,ty,tz) );
}

/// Set scaling part of matrix
/**	@warning Can be extracted back, but will ruin any preset matrix because it overwrites m11, m22 and m33
		@param v - 3D vector of XYZ scaling coefficients */
template<class T> void mat4<T>::SetScale( const vec3<T> &v ) {
	m11 = v.x;		m22 = v.y;		m33 = v.z;
}

/// Set scaling part of matrix (using constant scalar)
/**	@warning Can be extracted back, but will ruin any preset matrix because it overwrites m11, m22 and m33
		@param s - constant scalar */
template<class T> void mat4<T>::SetScale( const T &s ) {
	m11 = s;		m22 = s;		m33 = s;
}

/// Get scaling part of the matrix
/**	@return vec3 composed of m11, m22 and m33
		@warning May not be correct if the matrix is composed or not strictly scalar */
template<class T> vec3<T> mat4<T>::GetScale() const {
	return vec3<T>( m11, m22, m33 );
}

/// Apply scale (using XYZ coefficients)
/** @param v - vec3 of XYZ scalar coefficients */
template<class T> void mat4<T>::Scale( const vec3<T> &v ) {
	m11 *= v.x;		m22 *= v.y;		m33 *= v.z;
}

/// Apply scale (using constant scalar)
/** @param v - constant scalar */
template<class T> void mat4<T>::Scale( const T &v ) {
	m11 *= v;		m22 *= v;		m33 *= v;
}

/// SETUP MATRIX AS SCALAR (using XYZ coefficients)
/** @param v - vec3 of XYZ scalar coefficients */
template<class T> void mat4<T>::Scalar( const vec3<T> &v ) {
	Identity();
	SetScale( v );
}

/// SETUP MATRIX AS SCALAR using constant scalar)
/** @param v - constant scalar */
template<class T> void mat4<T>::Scalar( const T &v ) {
	Identity();
	SetScale( v );
}

/// SETUP MATRIX AS ROTATIONAL (using quaternion)
/** @param q - external quaternion */
template<class T> void mat4<T>::Rotational( const quat<T> &q ) {
	Identity();
	T xx  = q.x * q.x;	T xy  = q.x * q.y;	T xz  = q.x * q.z;
	T xw  = q.x * q.w;	T yy  = q.y * q.y;	T yz  = q.y * q.z;
	T yw  = q.y * q.w;	T zz  = q.z * q.z;	T zw  = q.z * q.w;
	m11  = (T)1 -	(T)2 * ( yy + zz );	m12  =			(T)2 * ( xy + zw );	m13  =			(T)2 * ( xz - yw );
	m21  =			(T)2 * ( xy - zw );	m22  = (T)1 -	(T)2 * ( xx + zz );	m23  =			(T)2 * ( yz + xw );
	m31  =			(T)2 * ( xz + yw );	m32  =			(T)2 * ( yz - xw );	m33  = (T)1 -	(T)2 * ( xx + yy );

	*this = this->Transposed();
}

/// SETUP MATRIX AS ROTATIONAL (using axis and angle)
/**	@param q - external vec3 axis
		@param a - angle in degrees */
template<class T> void mat4<T>::Rotational( const vec3<T> &q, const T &a ) {
	Identity();
	T rcos = cos( d2r(a) );
	T rsin = sin( d2r(a) );
	m11 =				rcos + q.x*q.x*((T)1-rcos);
	m21 =  q.z *	rsin + q.y*q.x*((T)1-rcos);
	m31 = -q.x *	rsin + q.z*q.x*((T)1-rcos);
	m12 = -q.z *	rsin + q.x*q.y*((T)1-rcos);
	m22 =				rcos + q.y*q.y*((T)1-rcos);
	m32 =  q.x *	rsin + q.z*q.y*((T)1-rcos);
	m13 =  q.y *	rsin + q.x*q.z*((T)1-rcos);
	m23 = -q.x *	rsin + q.y*q.z*((T)1-rcos);
	m33 =				rcos + q.z*q.z*((T)1-rcos);
}

/// SETUP MATRIX AS ROTATIONAL AROUND RIGHT (yaw)
/** @param x - angle in degrees */
template<class T> void mat4<T>::RotationalX( const T &x ) {
	Rotational( RIGHT, x );
}

/// SETUP MATRIX AS ROTATIONAL AROUND UP (turn)
/** @param x - angle in degrees */
template<class T> void mat4<T>::RotationalY( const T &x ) {
	Rotational( UP, x );
}

/// SETUP MATRIX AS ROTATIONAL AROUND FORWARD (roll)
/** @param x - angle in degrees */
template<class T> void mat4<T>::RotationalZ( const T &x ) {
	Rotational( FORWARD, x );
}

/// SETUP MATRIX AS SHEARING
/** @param vec3 - 3D direction * 1D power of shearing. vec3(0) is no shearing */
template<class T> void mat4<T>::Shearal( const vec3<T> &q ) {
	Identity();
	m21 = q.y;	m12 = q.x;	m13 = q.x;
	m31 = q.z;	m32 = q.z;	m23 = q.y;
}

/// Shear the matrix (this one uses accumulation)
/**	@param vec3 - 3D direction * 1D power of shearing. vec3(1) is no shearing
		@warning Using vec3(0) as powered direction will result in loss of orientation and wrong scaling */
template<class T> void mat4<T>::Shear( const vec3<T> &q ) {
	m21 *= q.y;	m12 *= q.x;	m13 *= q.x;
	m31 *= q.z;	m32 *= q.z;	m23 *= q.y;
	//TODO test - experimental
}

/// SETUP AS COMBINED TRANSFORMATION (orient, scale and translate )
/**	@param pos - 3D position
		@param sca - 3D scaling
		@param ori - 4D quaternion orientation */
template<class T> void mat4<T>::Make( const vec3<T> &pos, const vec3<T> &sca, const quat<T> &ori ) {
	Identity();
	mat4<T> translation;		translation.Translational( pos );
	mat4<T> scale;				scale.Scalar( sca );
	mat4<T> rotational;		rotational.Rotational( ori );

	*this	*=	rotational;
	*this	*=	scale;
	*this	*=	translation;
}

/// SETUP AS COMBINED TRANSFORMATION (scale, orient and translate )
/**	@param pos - 3D position
		@param ori - 4D quaternion orientation
		@param sca - 3D scaling */
template<class T> void mat4<T>::Make( const vec3<T> &pos, const quat<T> &ori, const vec3<T> &sca ) {
	Identity();
	mat4<T> translation;		translation.Translational( pos );
	mat4<T> scale;				scale.Scalar( sca );
	mat4<T> rotational;		rotational.Rotational( ori );

	*this	*=	scale;
	*this	*=	rotational;
	*this	*=	translation;
}

/// SETUP AS COMBINED TRANSFORMATION (orient, scale and translate )
/**	@param ori - 4D quaternion orientation
		@param pos - 3D position
		@param sca - 3D scaling */
template<class T> void mat4<T>::Make( const quat<T> &ori, const vec3<T> &pos, const vec3<T> &sca ) {
	Identity();
	mat4<T> translation;		translation.Translational( pos );
	mat4<T> scale;				scale.Scalar( sca );
	mat4<T> rotational;		rotational.Rotational( ori );

	*this	*=	scale;
	*this	*=	translation;
	*this	*=	rotational;
}

/// Set entries
/**	@param n11 - n44 - entries of the matrix */
template<class T> void mat4<T>::Set(	T n11, T n12, T n13, T n14,
													T n21, T n22, T n23, T n24,
													T n31, T n32, T n33, T n34,
													T n41, T n42, T n43, T n44	){
	m11 = n11; m12 = n12; m13 = n13; m14 = n14;
	m21 = n21; m22 = n22; m23 = n23; m24 = n24;
	m31 = n31; m32 = n32; m33 = n33; m34 = n34;
	m41 = n41; m42 = n42; m43 = n43; m44 = n44;
}

/// Identity...fy
template<class T> mat4<T> mat4<T>::Identity( ) {
	m11 = (T)1; m12 = (T)0; m13 = (T)0; m14 = (T)0;
	m21 = (T)0; m22 = (T)1; m23 = (T)0; m24 = (T)0;
	m31 = (T)0; m32 = (T)0; m33 = (T)1; m34 = (T)0;
	m41 = (T)0; m42 = (T)0; m43 = (T)0; m44 = (T)1;
	return *this;
}

/// Nullify
template<class T> void mat4<T>::Null( ) {
	m11 = (T)0; m12 = (T)0; m13 = (T)0; m14 = (T)0;
	m21 = (T)0; m22 = (T)0; m23 = (T)0; m24 = (T)0;
	m31 = (T)0; m32 = (T)0; m33 = (T)0; m34 = (T)0;
	m41 = (T)0; m42 = (T)0; m43 = (T)0; m44 = (T)0;
}

template<class T> vec3<T>		mat4<T>::GetViewVector	( bool column_major ) const {
	if( column_major )
		return vec3<T>( m13, m23, m33 );
	return vec3<T>( m31, m32, m33 );
}

template<class T> vec3<T>		mat4<T>::GetUpVector		( bool column_major ) const {
	if( column_major )
		return vec3<T>( m12, m22, m32 );
	return vec3<T>( m21, m22, m23 );
}

template<class T> vec3<T>		mat4<T>::GetRightVector	( bool column_major ) const {
	if( column_major )
		return vec3<T>( m11, m21, m31 );
	return vec3<T>( m11, m12, m13 );
}

template<class T> void			mat4<T>::SetViewVector	( const vec3<T> &view, 	bool column_major ) {
	if( column_major ) {
		m13 = view.x;
		m23 = view.y;
		m33 = view.z;
		return;
	}
	m31 = view.x;
	m32 = view.y;
	m33 = view.z;
}

template<class T> void			mat4<T>::SetUpVector		( const vec3<T> &up, 	bool column_major ) {
	if( column_major ) {
		m12 = up.x;
		m22 = up.y;
		m32 = up.z;
		return;
	}
	m21 = up.x;
	m22 = up.y;
	m23 = up.z;
}

template<class T> void			mat4<T>::SetRightVector	( const vec3<T> &right, bool column_major ) {
	if( column_major ) {
		m11 = right.x;
		m21 = right.y;
		m31 = right.z;
		return;
	}
	m11 = right.x;
	m12 = right.y;
	m13 = right.z;
}


// Instancing
template class mat4<real>;
//template class mat4<float>;
//template class mat4<double>;





																					/// //////////////////
																					/// MATRIX 3x3
																					/// //////////////////





/// Default constructor. Everything is qual to zero
template<class T> mat3<T>::mat3( ) :
	m11(0), m12(0), m13(0),
	m21(0), m22(0), m23(0),
	m31(0), m32(0), m33(0) { }

/// Entry by entry constructor. Row-major
/** @param n11-n44 - entries */
template<class T> mat3<T>::mat3(	T n11, T n12, T n13,
											T n21, T n22, T n23,
											T n31, T n32, T n33	) :
	m11(n11), m12(n12), m13(n13),
	m21(n21), m22(n22), m23(n23),
	m31(n31), m32(n32), m33(n33) { }

/// Enter from array of size [16]. Row-major
/** @param data - array holder */
template<class T> mat3<T>::mat3( const T *data ) :
	m11(data[0]), m12(data[1]), m13(data[2]),
	m21(data[4]), m22(data[5]), m23(data[6]),
	m31(data[8]), m32(data[9]), m33(data[10]) { }

/// Copy (from float)
/** @param m3x3 - external matrix */
	template<class T> mat3<T>::mat3( const mat3f	&m3x3	) :
	m11((T)m3x3.m11), m12((T)m3x3.m12), m13((T)m3x3.m13),
	m21((T)m3x3.m21), m22((T)m3x3.m22), m23((T)m3x3.m23),
	m31((T)m3x3.m31), m32((T)m3x3.m32), m33((T)m3x3.m33) { }

/// Copy (from double)
/** @param m3x3 - external matrix */
template<class T> mat3<T>::mat3( const mat3d	&m3x3 ) :
	m11((T)m3x3.m11), m12((T)m3x3.m12), m13((T)m3x3.m13),
	m21((T)m3x3.m21), m22((T)m3x3.m22), m23((T)m3x3.m23),
	m31((T)m3x3.m31), m32((T)m3x3.m32), m33((T)m3x3.m33) { }

/// Copy (from float)
/** @param m4x4 - external matrix */
template<class T> mat3<T>::mat3( const mat4f	&m4x4	) :
	m11((T)m4x4.m11), m12((T)m4x4.m12), m13((T)m4x4.m13),
	m21((T)m4x4.m21), m22((T)m4x4.m22), m23((T)m4x4.m23),
	m31((T)m4x4.m31), m32((T)m4x4.m32), m33((T)m4x4.m33) { }

/// Copy (from double)
/** @param m4x4 - external matrix */
template<class T> mat3<T>::mat3( const mat4d	&m4x4 ) :
	m11((T)m4x4.m11), m12((T)m4x4.m12), m13((T)m4x4.m13),
	m21((T)m4x4.m21), m22((T)m4x4.m22), m23((T)m4x4.m23),
	m31((T)m4x4.m31), m32((T)m4x4.m32), m33((T)m4x4.m33) { }

/// Construct from rows
/** @param row 1-4 - row vectors. row 4 default is vec4( 0, 0, 0, 1 ) */
template<class T> mat3<T>::mat3(	const vec3<T> &row0,
											const vec3<T> &row1,
											const vec3<T> &row2	 ) :
	m11(row0.x), m12(row0.y), m13(row0.z),
	m21(row1.x), m22(row1.y), m23(row1.z),
	m31(row2.x), m32(row2.y), m33(row2.z) { }

/// Construct from coordinate system unit vectors, position and scale
/**	@param - right, up, view - vectors of orientation
		@param - pos - translation. The default is vec3(0,0,0)
		@param - scale - scaling. The default is vec3(1,1,1) */
template<class T> mat3<T>::mat3(	const vec3<T> &right,
											const vec3<T> &up,
											const vec3<T> &view,
											const vec3<T> &scale	) :
	m11(right.x*scale.x), m12(up.x), m13(view.x),
	m21(right.y), m22(up.y*scale.y), m23(view.y),
	m31(right.z), m32(up.z), m33(view.z*scale.z) { }

/// Construct from quaternion, scaling and position
/**	@param - orient - orientation quaternion
		@param - scale - scaling. The default is vec3(1,1,1)
		@param - pos - translation. The default is vec3(0,0,0) */
template<class T> mat3<T>::mat3(	const quat<T> &orient, const vec3<T> &scale ) {
	quat<T> q = orient.NormalizedQ();
	T x2 = q.x * q.x;		T y2 = q.y * q.y;		T z2 = q.z * q.z;
	T xy = q.x * q.y;		T xz = q.x * q.z;		T yz = q.y * q.z;
	T wx = q.w * q.x;		T wy = q.w * q.y;		T wz = q.w * q.z;

	m11 = ((T)1.0 - (T)2.0 * (y2 + z2))*scale.x;	m12 = (T)2.0 * (xy - wz);							m13 = (T)2.0 * (xz + wy);
	m21 = (T)2.0 * (xy + wz);							m22 = ((T)1.0 - (T)2.0 * (x2 + z2))*scale.y;	m23 = (T)2.0 * (yz - wx);
	m31 = (T)2.0 * (xz - wy);							m32 = (T)2.0 * (yz + wx);							m33 = ((T)1.0 - (T)2.0 * (x2 + y2))*scale.z;
}



/// Multiplication operator (matrix * matrix)
/**	@param m2 - external matrix
		@return mat4 */
template<class T> mat3<T> mat3<T>::operator * ( const mat3<T> &m2	) const {
	mat3<T> r;
	r.m11 = m11 * m2.m11 + m12 * m2.m21 + m13 * m2.m31;
	r.m12 = m11 * m2.m12 + m12 * m2.m22 + m13 * m2.m32;
	r.m13 = m11 * m2.m13 + m12 * m2.m23 + m13 * m2.m33;

	r.m21 = m21 * m2.m11 + m22 * m2.m21 + m23 * m2.m31;
	r.m22 = m21 * m2.m12 + m22 * m2.m22 + m23 * m2.m32;
	r.m23 = m21 * m2.m13 + m22 * m2.m23 + m23 * m2.m33;

	r.m31 = m31 * m2.m11 + m32 * m2.m21 + m33 * m2.m31;
	r.m32 = m31 * m2.m12 + m32 * m2.m22 + m33 * m2.m32;
	r.m33 = m31 * m2.m13 + m32 * m2.m23 + m33 * m2.m33;

	return r;
}

/// Multiplication operator (matrix * vec3)
/**	@param v - 3D vector
		@return 3D vector */
template<class T> vec3<T> mat3<T>::operator * ( const vec3<T> &v ) const {
   return vec3<T>(
      m11 * v.x + m12 * v.y + m13 * v.z,
      m21 * v.x + m22 * v.y + m23 * v.z,
      m31 * v.x + m32 * v.y + m33 * v.z  );
}

/// Multiplication operator (matrix * vec4)
/**	@param v - 4D vector
		@return 4D vector */
template<class T> vec4<T> mat3<T>::operator * ( const vec4<T> &v ) const {
	return vec3<T>( *this * vec3<T>(v) );
}

/// Multiplication operator (matrix * scalar)
/**	@param scalar - 1D variable
		@return mat4 */
template<class T> mat3<T> mat3<T>::operator * ( const T &scalar ) const {
   return mat3<T>(
      m11 * scalar, m12 * scalar, m13 * scalar,
      m21 * scalar, m22 * scalar, m23 * scalar,
      m31 * scalar, m32 * scalar, m33 * scalar	);
}

/// Addition operator (matrix + matrix)
/**	@param m2 - external mat4
		@return mat4 */
template<class T> mat3<T> mat3<T>::operator + ( const mat3<T> &m2	) const {
   return mat3<T>(
      m11 + m2.m11, m12 + m2.m12, m13 + m2.m13,
      m21 + m2.m21, m22 + m2.m22, m23 + m2.m23,
      m31 + m2.m31, m32 + m2.m32, m33 + m2.m33	);
}

/// Subtraction operator (matrix - matrix)
/**	@param m2 - external mat4
		@return temporary mat4 */
template<class T> mat3<T> mat3<T>::operator - ( const mat3<T> &m2	) const {
   return mat3<T>(
      m11 - m2.m11, m12 - m2.m12, m13 - m2.m13,
      m21 - m2.m21, m22 - m2.m22, m23 - m2.m23,
      m31 - m2.m31, m32 - m2.m32, m33 - m2.m33	);
}

/// Self-multiplication operator (matrix *= matrix)
/**	@param m2 - external mat4
		@return temporary mat4 */
template<class T> mat3<T> mat3<T>::operator *= ( const mat3<T> &m2 ) const {
	return *this * m2;
}

/// Self-multiplication operator (matrix *= scalar)
/**	@param scalar - 1D var
		@return temporary mat4 */
template<class T> mat3<T> mat3<T>::operator *= ( const T &scalar ) const {
	return *this * scalar;
}

/// Equality operator (matrix == matrix)
/**	@param m2 - external mat4
		@return true if equal */
template<class T> bool mat3<T>::operator == ( const mat3<T> &m2 ) const {
	if( m11 == m2.m11	&&	m12 == m2.m12	&&	m13 == m2.m13	&&
		 m21 == m2.m21	&&	m22 == m2.m22	&&	m23 == m2.m23	&&
		 m31 == m2.m31	&&	m32 == m2.m32	&&	m33 == m2.m33	){
		return true;
	}
	return false;
}

/// Non-equality operator (matrix != matrix)
/**	@param m2 - external mat4
		@return true if not equal */
template<class T> bool mat3<T>::operator != ( const mat3<T> &m2 ) const {
	if(   m11 != m2.m11	||	m12 != m2.m12	||	m13 != m2.m13	||
			m21 != m2.m21	||	m22 != m2.m22	||	m23 != m2.m23	||
			m31 != m2.m31	||	m32 != m2.m32	||	m33 != m2.m33	){
		return true;
	}
	return false;
}

/// Row-major (Default) array filling. Row-major
/** @param *&array - holder to allocated array of size [9] */
template<class T> void mat3<T>::GetArray( T *arr ) const {
	arr[0] = m11; arr[1]	= m12; arr[2] = m13;
	arr[3] = m21; arr[4]	= m22; arr[5] = m23;
	arr[6] = m31; arr[7]	= m32; arr[8] = m33;
}

/// Column-major array filling. Column-major
/** @param *&array - holder to allocated array of size [16] */
template<class T> void mat3<T>::GetArrayCM( T *arr ) const {
	arr[0] = m11; arr[1]	= m21; arr[2] = m31;
	arr[3] = m12; arr[4]	= m22; arr[5] = m32;
	arr[6] = m13; arr[7]	= m23; arr[8] = m33;
}

/// Row-major (Default) array filling. Row-major
/** @param *&array - holder to allocated array of size [16] */
template<class T> void mat3<T>::GetArrayRM( T *arr ) const {
	GetArray( arr );
}

/// Get row as vec3
/**	@param row - row index row=[0;3]. Invalid row returns ZEROVEC
		@return vec4 */
template<class T> vec3<T> mat3<T>::GetRow( const uint16 &row ) const {
	if( row == 0 )
		return vec3<T>( m11, m12, m13 );
	else if( row == 1 )
		return vec3<T>( m21, m22, m23 );
	else if( row == 2 )
		return vec3<T>( m31, m32, m33 );
	else
		return vec3<T>( 0.f );
}

/// Get column as vec4
/**	@param col - column index col=[0;3]. Invalid column returns ZEROVEC
		@return vec4 */
template<class T> vec3<T> mat3<T>::GetCol( const uint16 &col ) const {
	if( col == 0 )
		return vec3<T>( m11, m21, m31 );
	else if( col == 1 )
		return vec3<T>( m12, m22, m32 );
	else if( col == 2 )
		return vec3<T>( m13, m23, m33 );
	else
		return vec3<T>( 0.f );
}

/// Get EULER angles from rotational matrix
/**	@return vec3 with turn, yaw, roll angles
		@warning matrix must be set as rotational before use */
template<class T> vec3<T> mat3<T>::GetEuler( ) {
	T D,C;
	T angle_y = D =  asin( m31);				/* Calculate Y-axis angle */
	T angle_x, angle_z;
	C           =  cos( angle_y );
	angle_y    =  r2d( angle_y );
	if ( fabs( C ) > (T)0.005 ){					/* Gimball lock? */
      T trx      =  m33 / C;							/* No, so get X-axis angle */
      T tryy      = -m23  / C;
      angle_x  =  r2d( atan2( tryy, trx ) );
      trx      =  m11 / C;							/* Get Z-axis angle */
      tryy      = -m12 / C;
      angle_z  =  r2d( atan2( tryy, trx ) );
   }
	else{													/* Gimball lock has occurred */
      angle_x  =  0;									/* Set X-axis angle to zero */
      T trx      =  m22;								/* And calculate Z-axis angle */
      T tryy      =  m21;
      angle_z  =  r2d( atan2( tryy, trx ) );
   }

   /* return only positive angles in [0,360] */
   if( angle_x < 0 ) angle_x += (T)360;
   if( angle_y < 0 ) angle_y += (T)360;
   if( angle_z < 0 ) angle_z += (T)360;

	return vec3<T>( angle_x, angle_y, angle_z );
}

/// Get transpose of this matrix
/** @return mat4 */
template<class T> mat3<T> mat3<T>::Transposed( void ) const {
    return mat3<T>(	m11, m21, m31,
							m12, m22, m32,
							m13, m23, m33	);
}

/// Set scaling part of matrix
/**	@warning Can be extracted back, but will ruin any preset matrix because it overwrites m11, m22 and m33
		@param v - 3D vector of XYZ scaling coefficients */
template<class T> void mat3<T>::SetScale( const vec3<T> &v ) {
	m11 = v.x;		m22 = v.y;		m33 = v.z;
}

/// Set scaling part of matrix (using constant scalar)
/**	@warning Can be extracted back, but will ruin any preset matrix because it overwrites m11, m22 and m33
		@param s - constant scalar */
template<class T> void mat3<T>::SetScale( const T &s ) {
	m11 = s;		m22 = s;		m33 = s;
}

/// Get scaling part of the matrix
/**	@return vec3 composed of m11, m22 and m33
		@warning May not be correct if the matrix is composed or not strictly scalar */
template<class T> vec3<T> mat3<T>::GetScale( ) const {
	return vec3<T>( m11, m22, m33 );
}

/// Apply scale (using XYZ coefficients)
/** @param v - vec3 of XYZ scalar coefficients */
template<class T> void mat3<T>::Scale( const vec3<T> &v ) {
	m11 *= v.x;	m22 *= v.y;	m33 *= v.z;
}

/// Apply scale (using constant scalar)
/** @param v - constant scalar */
template<class T> void mat3<T>::Scale( const T &v ) {
	m11 *= v;	m22 *= v;	m33 *= v;
}

/// SETUP MATRIX AS SCALAR (using XYZ coefficients)
/** @param v - vec3 of XYZ scalar coefficients */
template<class T> void mat3<T>::Scalar( const vec3<T> &v ) {
	Identity();
	SetScale( v );
}

/// SETUP MATRIX AS SCALAR using constant scalar)
/** @param v - constant scalar */
template<class T> void mat3<T>::Scalar( const T &v ) {
	Identity();
	SetScale( v );
}

/// SETUP MATRIX AS ROTATIONAL (using quaternion)
/** @param q - external quaternion */
template<class T> void mat3<T>::Rotational( const quat<T> &q ) {
	Identity();
	T xx  = q.x * q.x;	T xy  = q.x * q.y;	T xz  = q.x * q.z;
	T xw  = q.x * q.w;	T yy  = q.y * q.y;	T yz  = q.y * q.z;
	T yw	= q.y * q.w;	T zz  = q.z * q.z;	T zw  = q.z * q.w;
	m11  = (T)1 -	(T)2 * ( yy + zz );	m12  =			(T)2 * ( xy + zw );	m13  =			(T)2 * ( xz - yw );
	m21  =			(T)2 * ( xy - zw );	m22  = (T)1 -	(T)2 * ( xx + zz );	m23  =			(T)2 * ( yz + xw );
	m31  =			(T)2 * ( xz + yw );	m32  =			(T)2 * ( yz - xw );	m33  = (T)1 -	(T)2 * ( xx + yy );

	*this = this->Transposed();
}

/// SETUP MATRIX AS ROTATIONAL (using axis and angle)
/**	@param q - external vec3 axis
		@param a - angle in degrees */
template<class T> void mat3<T>::Rotational( const vec3<T> &q, const T &a ) {
	Identity();
	T rcos = cos( d2r(a) );
	T rsin = sin( d2r(a) );
	m11 =				rcos + q.x*q.x*((T)1-rcos);
	m21 =  q.z *	rsin + q.y*q.x*((T)1-rcos);
	m31 = -q.x *	rsin + q.z*q.x*((T)1-rcos);
	m12 = -q.z *	rsin + q.x*q.y*((T)1-rcos);
	m22 =				rcos + q.y*q.y*((T)1-rcos);
	m32 =  q.x *	rsin + q.z*q.y*((T)1-rcos);
	m13 =  q.y *	rsin + q.x*q.z*((T)1-rcos);
	m23 = -q.x *	rsin + q.y*q.z*((T)1-rcos);
	m33 =				rcos + q.z*q.z*((T)1-rcos);
}

/// SETUP MATRIX AS ROTATIONAL AROUND RIGHT (yaw)
/** @param x - angle in degrees */
template<class T> void mat3<T>::RotationalX( const T &x ) {
	Rotational( RIGHT, x );
}

/// SETUP MATRIX AS ROTATIONAL AROUND UP (turn)
/** @param x - angle in degrees */
template<class T> void mat3<T>::RotationalY( const T &x ) {
	Rotational( UP, x );
}

/// SETUP MATRIX AS ROTATIONAL AROUND FORWARD (roll)
/** @param x - angle in degrees */
template<class T> void mat3<T>::RotationalZ( const T &x ) {
	Rotational( FORWARD, x );
}

/// SETUP MATRIX AS SHEARING
/** @param vec3 - 3D direction * 1D power of shearing. vec3(0) is no shearing */
template<class T> void mat3<T>::Shearal( const vec3<T> &q ) {
	Identity();
	m21 = q.y;	m12 = q.x;	m13 = q.x;
	m31 = q.z;	m32 = q.z;	m23 = q.y;
}

/// Shear the matrix (this one uses accumulation)
/**	@param vec3 - 3D direction * 1D power of shearing. vec3(1) is no shearing
		@warning Using vec3(0) as powered direction will result in loss of orientation and wrong scaling */
template<class T> void mat3<T>::Shear( const vec3<T> &q ) {
	m21 *= q.y;	m12 *= q.x;	m13 *= q.x;
	m31 *= q.z;	m32 *= q.z;	m23 *= q.y;
	//TODO test - experimental
}

/// SETUP AS COMBINED TRANSFORMATION (scale, orient and translate )
/**	@param pos - 3D position
		@param ori - 4D quaternion orientation
		@param sca - 3D scaling */
template<class T> void mat3<T>::Make( const quat<T> &ori, const vec3<T> &sca ) {
	Identity();
	mat3<T> scale;				scale.Scalar( sca );
	mat3<T> rotational;		rotational.Rotational( ori );

	*this	*=	scale;
	*this	*=	rotational;
}

/// Set entries
/**	@param n11- n33 - entries to set */
template<class T> void mat3<T>::Set(	T n11, T n12, T n13,
													T n21, T n22, T n23,
													T n31, T n32, T n33	){
	m11 = n11; m12 = n12; m13 = n13;
	m21 = n21; m22 = n22; m23 = n23;
	m31 = n31; m32 = n32; m33 = n33;
}

/// Identity... fy
template<class T> void mat3<T>::Identity( ) {
	m11 = (T)1; m12 = (T)0; m13 = (T)0;
	m21 = (T)0; m22 = (T)1; m23 = (T)0;
	m31 = (T)0; m32 = (T)0; m33 = (T)1;
}

/// Nullify
template<class T> void mat3<T>::Null( ) {
	m11 = (T)0; m12 = (T)0; m13 = (T)0;
	m21 = (T)0; m22 = (T)0; m23 = (T)0;
	m31 = (T)0; m32 = (T)0; m33 = (T)0;
}

template<class T> vec3<T>		mat3<T>::GetViewVector	( bool column_major ) const {
	if( column_major )
		return vec3<T>( m13, m23, m33 );
	return vec3<T>( m31, m32, m33 );
}

template<class T> vec3<T>		mat3<T>::GetUpVector		( bool column_major ) const {
	if( column_major )
		return vec3<T>( m12, m22, m32 );
	return vec3<T>( m21, m22, m23 );
}

template<class T> vec3<T>		mat3<T>::GetRightVector	( bool column_major ) const {
	if( column_major )
		return vec3<T>( m11, m21, m31 );
	return vec3<T>( m11, m12, m13 );
}

template<class T> void			mat3<T>::SetViewVector	( const vec3<T> &view, 	bool column_major ) {
	if( column_major ) {
		m13 = view.x;
		m23 = view.y;
		m33 = view.z;
		return;
	}
	m31 = view.x;
	m32 = view.y;
	m33 = view.z;
}

template<class T> void			mat3<T>::SetUpVector		( const vec3<T> &up, 	bool column_major ) {
	if( column_major ) {
		m12 = up.x;
		m22 = up.y;
		m32 = up.z;
		return;
	}
	m21 = up.x;
	m22 = up.y;
	m23 = up.z;
}

template<class T> void			mat3<T>::SetRightVector	( const vec3<T> &right, bool column_major ) {
	if( column_major ) {
		m11 = right.x;
		m21 = right.y;
		m31 = right.z;
		return;
	}
	m11 = right.x;
	m12 = right.y;
	m13 = right.z;
}

// Instancing
template class mat3<real>;
//template class mat3<float>;
//template class mat3<double>;




